import sys
import os
import time

from socf import conf
from socf.client import local
from socf.client import service
from socf.services.constants import *

from WSDiscovery import *

def __convertServiceListToMap(list):
    map = {}
    for entry in list:
        map[entry[2]] = entry[1].__str__()
    return map
    
mineList = []
masterService = None
startPosition = None

def mineExists(mine):
    for m in mineList:
        if mine == m:
            return True
    return False
    
def addMine(mine):
    mineList.append(mine)
    
def getMineList():
    return mineList

def mine_detect(startPosition):
    #creates configuration context
    configContext = conf.ConfigurationContext(os.path.join(os.getcwd(),  "conf_master"))
    #get service stub provider
    localProvider = local.LocalObjectStubProvider.createProvider(configContext, "dbus")
    #contact the discovery service
    discoveryStub = localProvider.getServiceClientStub(DISCOVERY_SERVICE_TYPE, "dbus:laf.socf2/server/discoveryService")
    servicesMap = __convertServiceListToMap(discoveryStub.getCurrentServicesList())
    
    nav = localProvider.getServiceClientStub(NAV_SERVICE_TYPE, servicesMap["Navigation_Simulated1"])
    sonar_front = localProvider.getServiceClientStub(SONAR_SERVICE_TYPE, servicesMap["Sonar_Simulated_Front1"])
    sonar_right = localProvider.getServiceClientStub(SONAR_SERVICE_TYPE, servicesMap["Sonar_Simulated_Right1"])
    compass = localProvider.getServiceClientStub(COMPASS_SERVICE_TYPE, servicesMap["Compass_Simulated1"])
    gps = localProvider.getServiceClientStub(GPS_SERVICE_TYPE, servicesMap["GPS_Simulated1"])
    md = localProvider.getServiceClientStub(METAL_DETECTOR_SERVICE_TYPE, servicesMap["MetalDetector_Simulated1"])
    
    nav.moveLeft()
    nav.moveUp()
    print "running.."
    
    #goto the start position
    while True:
        loc = gps.getLocation()
        if loc[0] >= int(startPosition):
            nav.moveRight()
            break
    
    while True:
        mines = md.getMines()
        if len(mines) > 0:
            for mine in mines:
                if not mineExists(mine):
                    addMine(mine)
                    masterService.mine_found(str(mine),  "Worker-Robot1")
                    print "Mine found at " + "[" + str(mine) + "]"
        if is_obstacle(sonar_front):
            if is_front(compass):
                nav.moveLeft()
                time.sleep(0.15)
                nav.moveLeft()
            elif is_back(compass):
                if is_obstacle(sonar_right):
                    nav.stop()
                    break
                else:
                    nav.moveRight()
                    time.sleep(0.15)
                    nav.moveRight()
    
def is_front(compass):
    return compass.getDirection() == 0.0
    
def is_back(compass):
    return compass.getDirection() == 180.0
    
def is_obstacle(sonar):
    dist = sonar.getDistance()
    if dist < 3:
        return 1
    else:
        return 0

class MineWorkerService:

    def registerMaster(self,  serviceUrl, pos):
        """register the master service to notify them and start work at the specified position.

##wsdl:xsd:string, xsd:string, xsd:string
"""
        global masterService
        masterService = service.ServiceClient(serviceUrl)
        print "Master service registered at " + serviceUrl
        global startPosition
        startPosition = pos
        print "starting work at position: " + str(startPosition)        
        return ""
        
if __name__=="__main__":
    #add service
    mws = MineWorkerService()
    ss = service.ServiceContainer('192.168.2.253', 8001)
    serviceUrl = ss.addServiceObject(mws,  "mine_worker")
    ss.start()
    
    #for discovery
    wsd = WSDiscovery()
    wsd.start()
    type = QName("socf", "mineworker")
    wsd.publishService(types=[type], scopes=[], xAddrs=[serviceUrl])
    
    print "\nstarted.."
    
    #do work
    try:
        while startPosition == None:
            time.sleep(0.2)            
        mine_detect(startPosition)
        #signal the master robot
        masterService.mine_found("FINISHED",  "Worker-Robot1")
        wsd.stop()
        ss.stop()
    except KeyboardInterrupt:
        print "Shutdown.."
        wsd.stop()
        ss.stop()
    
